package edu.wpi.first.wpilibj.frc;

import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.mechanism.BridgeArm;
import edu.wpi.first.wpilibj.mechanism.PitchingMechanism;
import edu.wpi.first.wpilibj.op.Op;
import edu.wpi.first.wpilibj.op.Teleop12;
import edu.wpi.first.wpilibj.positioning.Coordinates;
import edu.wpi.first.wpilibj.positioning.Path;
import edu.wpi.first.wpilibj.positioning.PositionHandler;
import edu.wpi.first.wpilibj.util.ArraySet;
import edu.wpi.first.wpilibj.util.Rectangle3D;
import edu.wpi.first.wpilibj.util.Removeable;
import java.awt.geom.Rectangle2D;
import java.util.Iterator;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.ThreadFactory;
import java.util.concurrent.TimeUnit;

/**
 * Our robotic lump of duct tape, phlegm, and love.
 * @author Dylan Frese
 */
public class Robot extends SimpleRobot {

    /**
     * The time (in milliseconds) between each update of the runnables
     * in the tickers {@link ArraySet}.
     */
    public static final long TICKTIME = 100;
    /**
     * We have one robot. This isn't SINGLETON, but we have quick access to
     * our robot.
     */
    public static final Robot robot = new Robot();
    
    
    public final VirtualField currentField = new VirtualField(9999999, 9999999, new Rectangle3D[]{
        
            }, new Rectangle3D[]{
                
            });
    /**
     * This stores where the robot is as a {@link PositionHandler} it will be used
     * when the robot has to travel to a point on the field (It would have to
     * know where it is), and updated constantly with new input from the
     * sensors.
     */
    private PositionHandler where = new PositionHandler(currentField);
    /**
     * This is a set of {@link Runnable}s, which are run at a regular interval.
     * The set is iterated through every TICKTIME milliseconds.
     */
    private volatile ArraySet<Runnable> tickers = new ArraySet<Runnable>();
    /**
     * A {@link VirtualField} to represent the robots current surroundings.
     */
    
    public Removeable addTicker(final Runnable r){
        tickers.add(r);
        return new Removeable() {

            @Override
            public void remove() {
                tickers.remove(r);
            }
        };
    }
    
    /**
     * This is our spinny ball shooty thingy we use to get points.
     * Created on channel five action news.
     */
    public PitchingMechanism pitchingMachine = new PitchingMechanism(5);
    /**
     * Our bridge arm to raise, and lower, our bridges.
     */
    public BridgeArm bridgeArm = new BridgeArm(99);

    /**
     * Will not actually be called. In here for simple compile-tests.
     * Will be removed. Ignore. Please. The less you know the better.
     */
    public static void main(String[] args) {
        new Robot().startCompetition();
        
    }

    /**
     * Get where the robot is.
     * @return A {@link PositionHandler} that represents the location of the 
     * robot in coordinates.
     */
    public PositionHandler getWhere() {
        return where;
    }

    /**
     * Get a rectangle of the robot's presence, in Cartesian coordinates as is
     * in relation to the field.
     * @return the rectangle of spacetime (excluding time) that is occupied by
     * the duct-tape that our robot hopes to be.
     */
    public Rectangle2D getRectangle() {
        throw new UnsupportedOperationException();
//      return new edu.wpi.first.wpilibj.util.Rectangle2D(x, y, width, height);
    }
    /**
     * A {@link ScheduledExecutorService} to run things at times. Basically.
     */
    public static final ScheduledExecutorService executerService;

    //Initiliaze the executerService
    static {
        //Set it equal to a new Scheduled Thread Pool, with the size of three (Meaning three, parallel, threads).
        //We're also creating it with our own thread factory.
        executerService = Executors.newScheduledThreadPool(3, new ThreadFactory() {

            /**
             * {@inheritDoc}
             */
            @Override
            public Thread newThread(Runnable r) {
                //Create a new thread
                Thread t = new Thread(r);
                //Set it's name to be fancay.
                t.setName("Thread Pool Thread, Running " + r);
                return t;
            }
        });
    }
    /**
     * This is what controls the robot. The {@link Op} takes input, and does the
     * things. It can either be Teleop or Autop. (Autoop? AutoOp?). This is
     * called periodically.
     */
    private Op roboOp;

    /**
     * {@inheritDoc}
     */
    public void startCompetition() {
        //Create our drive on the channels 1 and 2.
        drive = new RobotDrive(1, 2);
        //Right now, we're using our Teleop12 to do the things. This will start
        //with AutoOp (Autop?) in the future.
        roboOp = Teleop12.getInstance();
        //Add a new ticker (Something called every TICKTIME milliseconds)
        //To tick our robot's op. Kinky.
        tickers.add(new Runnable() {

            @Override
            public void run() {
                roboOp.tick();
            }
        });
        //Create a new Runnable to be executed every TICKTIME MILLISECONDS
        //(Starting now) to go through and run the tickers.
        executerService.scheduleAtFixedRate(new Runnable() {

            @Override
            public void run() {
                for (Iterator<Runnable> it = tickers.iterator(); it.hasNext();) {
                    it.next().run();
                }
            }
        }, 0, TICKTIME, TimeUnit.MILLISECONDS);
    }
    public RobotDrive drive;

    /**
     * Drive the robot to the specified {@link Coordinates}.
     * If the coordinates need a different heading, the robot will turn
     * to at the speed of the Coordinates
     * @param c The coordinates that the robot will drive to
     */
    public void gotoCoords(Coordinates c) {
        gotoCoords(c, c.getSpeed());
    }

    /**
     * Drive the robot to the specified {@link Coordinates}.
     * If the coordinates need a different heading, the robot will turn
     * to at the speed of 'rotspeed'
     * @param c The coordinates that the robot will drive to
     * @param rotspeed The rotational speed the robot will use to turn.
     */
    public void gotoCoords(final Coordinates c, final double rotspeed) {
        final double angle = Math.atan2(c.getY() - where.getAsCoordinates().getY(), c.getX() - where.getAsCoordinates().getX());
        final double speed = Math.abs(c.getSpeed());
        final double rotationspeed = Math.abs(rotspeed);
        //Turn to face the point
        rotToAngle(angle, rotationspeed);
        //Drive forward, at the speed.
        while (!getRectangle().contains(c.getX(), c.getY())) {
            drive.tankDrive(speed, speed);
        }
    }

    /**
     * Rotate to the yaw of the Coordinates 'c'.
     * Does NOT rotate so that the robot will face the point
     * specified by the X/Y of the coordinates from the current position.
     * @param c The coordinates to use to specify the angle of turning.
     */
    public void rotToCoords(Coordinates c) {
        rotToAngle(c.getYaw(), c.getSpeed());
    }

    /**
     * Rotate the robot to the angle of 'yaw',
     * rotating at 'speed'.
     * @param yaw The angle to which to rotate.
     * @param speed The speed of the rotation (Motor speed; -1.0 to 1.0)
     */
    public void rotToAngle(double yaw, double speed) {
        //Only will be used if wheels cannot rotate
        final double oldyaw = where.getYaw();
        final boolean right = getRight(yaw, oldyaw);
        while (!inRange(yaw, where.getYaw())) {
            if (right) {
                drive.tankDrive(Math.abs(speed), 0);
            } else {
                drive.tankDrive(0, Math.abs(speed));
            }
        }
    }

    /**
     * Gets whether the robot should turn right or left to reach a certain
     * angle
     * @param yaw The desired angle
     * @param oldyaw The current angle
     * @return 'True' if right, 'False' if left.
     */
    private boolean getRight(double yaw, double oldyaw) {
        boolean right;
        if (yaw > 0 && oldyaw > 0) {
            right = yaw > oldyaw;
        } else if (yaw < 0 && oldyaw < 0) {
            right = oldyaw > yaw;
        } else {
            right = !getRight(Math.abs(yaw), Math.abs(oldyaw));
        }
        return right;
    }

    /**
     * Tests if the yaw1 is within reasonable range of yaw2.
     * @param ang1 First angle in degrees
     * @param ang2 Second angle in degrees
     * @return if the angles (Or, actually, numbers... really...) are within
     * reasonable range. (Close enough!)
     */
    private boolean inRange(double ang1, double ang2) {
        return Math.abs(ang1) - Math.abs (ang2) < 1;
    }

    /**
     * Have the robot follow the {@link Path}, or list of {@link Coordinates}.
     * This method does NOT loop, and is safe to call from with in a periodic.
     * @param p 
     */
    public void followPath(final Path p) {
        tickers.add(new Runnable() {

            @Override
            public void run() {
                gotoCoords(p.next());
                if (!p.hasNext()) {
                    tickers.remove(this);
                }
            }
        });
    }
}
